/*
 * region_search.cpp
 *
 *  Created on: 03.02.2013
 *      Author: Maksim Lazeba
 */

#include "region_search.h"
#include <algorithm>

typedef std::vector<point_t> points_t;
typedef region_tree::tree_node tree_node;
typedef region_tree::improved_tree_node improved_tree_node;

bool point_t::smaller(const point_t &b, int dim) const
{
	if (dim >= dimensions)
		return false;
	return x[dim] < b.x[dim];
}

int point_t::cmp(double d, int dim) const
{
	if (dim >= dimensions)
		return 0;
	double p = x[dim];
	if (p == d)
		return 0;
	return p < d ? -1 : 1;
}

bool rect_t::contains(const point_t &p) const
{
	for (int dim = 0; dim < p.dimensions; dim++)
	{
		if (p.smaller(lb, dim) || rt.smaller(p, dim))
			return false;
	}
	return true;
}

tree_node::tree_node(const point_t &p, int dim) :
		left(NULL), right(NULL), leaf(true), dim(dim), next_dim_root(NULL)
{
	lb = p.x[dim];
	rb = p.x[dim];
	points.push_back(p);
}

struct points_comparator
{
	int dim;

	points_comparator(int d) :
			dim(d)
	{
	}

	bool operator()(const point_t &a, const point_t &b)
	{
		return a.smaller(b, dim);
	}
};

void merge(points_t &left, points_t &right, points_t &to,
		points_comparator less)
{
	points_t::iterator a = left.begin();
	points_t::iterator b = right.begin();

	while (true)
	{
		if (a < left.end())
			if (b < right.end())
				if (less(*a, *b))
				{
					to.push_back(*a);
					a++;
				}
				else
				{
					to.push_back(*b);
					b++;
				}
			else
			{
				to.push_back(*a);
				a++;
			}
		else if (b < right.end())
		{
			to.push_back(*b);
			b++;
		}
		else
			break;
	}
}

template<typename T> T* build_tree(points_t &points, size_t l, size_t r,
		int dim)
{
	if (r - l == 0)
		return NULL;
	if (r - l == 1)
		return new T(points[l], dim);
	size_t m = (l + r) / 2;

	tree_node *left = build_tree<T>(points, l, m, dim);
	tree_node *right = build_tree<T>(points, m, r, dim);

	return new T(left, right, dim);
}

tree_node::tree_node(tree_node *left, tree_node *right, int dim) :
		left(left), right(right), leaf(false), lb(left->lb), rb(right->rb), dim(
				dim)
{
	merge(left->points, right->points, points, points_comparator(dim + 1));

	if (dim + 3 < points[0].dimensions)
		next_dim_root = build_tree<tree_node>(points, 0, points.size(),
				dim + 1);
	else if (dim + 3 == points[0].dimensions)
		next_dim_root = build_tree<improved_tree_node>(points, 0, points.size(),
				dim + 1);
	else
		next_dim_root = NULL;
}

improved_tree_node::improved_tree_node(tree_node *left, tree_node *right,
		int dim) :
		tree_node(left, right, dim)
{
	int llb = 0, lub = 0, rlb = 0, rub = 0;
	improved_tree_node *l = dynamic_cast<improved_tree_node*>(left);
	improved_tree_node *r = dynamic_cast<improved_tree_node*>(right);
	for (int i = 0; i < (int) points.size(); i++)
	{
		while (l->list[llb] < points[i])
			llb++;
		while (l->list[lub] < points[i])
			lub++;
		while (r->list[rlb] < points[i])
			rlb++;
		while (r->list[rub] < points[i])
			rub++;
		list.push_back(list_node(points[i], llb, lub, rlb, rub, i));
	}
	list.push_back(
			list_node(left->points.size(), right->points.size(), list.size()));
}

improved_tree_node::improved_tree_node(const point_t &p, int dim) :
		tree_node(p, dim)
{
	list.push_back(list_node(p, 0, 0, 0, 0, list.size()));
	list.push_back(list_node(0, 0, list.size()));
}

region_tree::region_tree(points_t &p, int d) :
		points(p), dimensions(d)
{
	points_comparator comp(0);
	std::sort(points.begin(), points.end(), comp);
	if (dimensions > 1)
		if (dimensions > 2)
			root = build_tree<tree_node>(points, 0, points.size(), 0);
		else
			root = build_tree<improved_tree_node>(points, 0, points.size(), 0);
	else
		root = NULL;
}

region_tree::~region_tree()
{
	delete root;
}

tree_node::~tree_node()
{
	delete left;
	delete right;
	delete next_dim_root;
}

improved_tree_node::~improved_tree_node()
{
}

void region_tree::tree_node::add_from_region(const rect_t &rect,
		std::vector<point_t>& ans) const
{
	int dim = this->dim;
	const point_t &left = rect.lb;
	const point_t &right = rect.rt;
	if (left.cmp(this->rb, dim) == 1 || right.cmp(this->lb, dim) == -1)
		return;
	if (left.cmp(this->lb, dim) < 1 && right.cmp(this->rb, dim) > -1)
	{
		if (this->leaf)
		{
			if (rect.contains(this->points[0]))
				ans.push_back(this->points[0]);
			return;
		}
		this->next_dim_root->add_from_region(rect, ans);
		return;
	}

	this->left->add_from_region(rect, ans);
	this->right->add_from_region(rect, ans);
}

void region_tree::improved_tree_node::add_from_region(const rect_t &rect,
		points_t &ans) const
{
	int dim = this->dim;
	const point_t &left = rect.lb;
	const point_t &right = rect.rt;
	if (left.cmp(this->rb, dim) == 1 || right.cmp(this->lb, dim) == -1)
		return;

	auto from = std::lower_bound(this->list.begin(), this->list.end() - 1,
			list_node(rect.lb));
	auto to = std::upper_bound(this->list.begin(), this->list.end() - 1,
			list_node(rect.rt));
	add_from_region_improved(rect, from->position, to->position, ans);
}

void region_tree::improved_tree_node::add_from_region_improved(
		const rect_t &rect, int lb_cur, int ub_cur, points_t &ans) const
{
	int dim = this->dim;
	const point_t &left = rect.lb;
	const point_t &right = rect.rt;
	if (left.cmp(this->rb, dim) == 1 || right.cmp(this->lb, dim) == -1)
		return;
	if (left.cmp(this->lb, dim) < 1 && right.cmp(this->rb, dim) > -1)
	{
		for (int i = lb_cur; i < ub_cur; i++)
		{
			ans.push_back(*list[i].point);
		}
		return;
	}
	if (leaf)
		return;
	this->left->add_from_region_improved(rect, list[lb_cur].left_lb,
			list[ub_cur].left_ub, ans);
	this->right->add_from_region_improved(rect, list[lb_cur].right_lb,
			list[ub_cur].right_ub, ans);

}

bool region_tree::improved_tree_node::list_node::operator<(
		const list_node& b) const
{
	if (end)
		return false;
	return point->smaller(*b.point, point->dimensions - 1);
}

bool region_tree::improved_tree_node::list_node::operator<(
		const point_t& b) const
{
	if (end)
		return false;
	return point->smaller(b, point->dimensions - 1);
}

std::vector<point_t> region_tree::get_points_in_rect(const rect_t &rect) const
{
	if (dimensions > 1)
	{
		points_t ans;
		root->add_from_region(rect, ans);
		return ans;
	}
	else
	{
		points_comparator comp(0);
		auto from = std::lower_bound(points.begin(), points.end(), rect.lb,
				comp);
		auto to = std::upper_bound(points.begin(), points.end(), rect.rt, comp);
		return std::vector<point_t>(from, to);
	}
}
